package com.squeekeek.util.qrcode.Core;

import java.util.Arrays;
import java.util.Collections;
import java.util.List;

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;

public class QRImageCorrection {
	
	/**
	 * <p>
	 * Reorders the indices of the finder pattern centers such that the
	 * indices start at the upper-left finder pattern and proceed counter-clockwise
	 * </p>
	 * <p>
	 * The function reorders the finder pattern centers contained in
	 * <code>finderCenters</code> such that the state that these finder centers are
	 * ordered are set to true. The location of these centers can therefore be
	 * accessed using the function {@link QRFinderPattern.getPosition}.
	 * </p>
	 * 
	 * @param finderCenters The finder pattern centers to reorder
	 * @return The reordered finder pattern centers
	 */
	public static QRFinderPattern reorderOrientation(QRFinderPattern finderCenters) {
		int maxIdx = -1;
		int ulIdx, urIdx;
		double maxLen = -1;
		double vecDet;
		double nowLen;
		List <Point> points = Arrays.asList(finderCenters.getArray());
		
		// Get longest length between the three points
		for(int i = 0; i < 3; i++) {
			nowLen = Core.norm(OpenCVExtras.subtractPoints2D(points.get(i), points.get((i + 1) % 3)), Core.NORM_L2);
			
			if((int) 1000 * (maxLen - nowLen) < 0) {
				maxLen = nowLen;
				maxIdx = i;
			}
		}
		
		// Get upper-left finder pattern
		ulIdx = (maxIdx + 2) % 3;
		
		// Get upper-right and lower-left finder patterns using cross product
		vecDet = OpenCVExtras.detVectors2D(OpenCVExtras.subtractPoints2D(points.get((ulIdx + 1) % 3), points.get(ulIdx)), OpenCVExtras.subtractPoints2D(points.get((ulIdx + 2) % 3), points.get(ulIdx)));
		
		if((int) Math.signum(vecDet) == -1) {
			// Negative determinant
			urIdx = (ulIdx + 2) % 3;
			
		}
		else {
			urIdx = (ulIdx + 1) % 3;
		}
		
		
		// Reorder points such that the upper-left finder pattern is
		// in index zero
		Collections.swap(points, ulIdx, 0);
		
		if(urIdx == 0) {
			urIdx = ulIdx;
		}
		
		Collections.swap(points, urIdx, 2);
		
		return new QRFinderPattern(points, true);
	}
	
	/**
	 * <p>
	 * Gets the rotation matrix associated with an aligned set of
	 * finder patterns.
	 * </p>
	 * 
	 * @param finderCenters An instance of the three finder patterns of a
	 * QR Code
	 * @return A rotation matrix
	 */
	public static Mat getRotationMat(QRFinderPattern finderCenters) {
		Point ll = finderCenters.getPosition(QRFinderPattern.POS_LL);
		Point ur = finderCenters.getPosition(QRFinderPattern.POS_UR);
		Point center;
		
		center = new Point( Math.abs(ll.x - ur.x) / 2 + Math.min(ll.x, ur.x), Math.abs(ll.y - ur.y) / 2 + Math.min(ll.y, ur.y) );
		
		return Imgproc.getRotationMatrix2D(center, getRotationAngle(finderCenters), 1);
	}
	
	private static double getRotationAngle(QRFinderPattern finderCenters) {
		int slopeSign;
		double maxSlope;
		
		if(finderCenters.getReordered() == false) {
			return -1;
		}
		
		maxSlope = (finderCenters.getPosition(QRFinderPattern.POS_LL).y - finderCenters.getPosition(QRFinderPattern.POS_UR).y) / (finderCenters.getPosition(QRFinderPattern.POS_LL).x - finderCenters.getPosition(QRFinderPattern.POS_UR).x);
		
		if((int) (1000 * Math.abs(maxSlope)) < 1000) {
			slopeSign = 1;
		}
		else {
			slopeSign = -1;
		}

		return 90 * slopeSign + Math.toDegrees(Math.atan(slopeSign * (maxSlope - 1) / (1 + maxSlope)));
	}
	
	public static Mat rotateImg(Mat inImg, Mat rotMat) {
		Mat outImg = new Mat(inImg.size(), CvType.CV_8UC1);
		
		Imgproc.warpAffine(inImg, outImg, rotMat, inImg.size(), Imgproc.INTER_LINEAR);
		
		return outImg;
	}
	
	public static QRFinderPattern rotateFinderPatterns(QRFinderPattern finderCenters, Mat rotMat) {
		Point[] myCenters = finderCenters.getArray();
		
		for(int i = 0; i < 3; i++) {
			myCenters[i] = rotatePoint(myCenters[i], rotMat);
		}
		
		finderCenters.setValue(myCenters);
		
		return finderCenters;
	}
	
	public static Point rotatePoint(Point myPoint, Mat rotMat) {
		Mat ptTrans = new Mat(3, 1, CvType.CV_64FC1);
		
		Core.gemm(rotMat, OpenCVExtras.pointToMat3D(myPoint), 1.0, new Mat(), 0.0, ptTrans);
		
		return OpenCVExtras.mat3DToPoint(ptTrans);
	}
	
	/**
	 * <p>
	 * Gets an affine transformation matrix associated with a rotated
	 * set of finder patterns.
	 * </p>
	 * 
	 * @param finderCenters An instance of the three finder patterns of a QR Code
	 * @return An affine transformation matrix
	 */
	public static Mat getAffineMat(QRFinderPattern finderCenters) {
		double codeWidth;
		MatOfPoint2f srcMat = new MatOfPoint2f();
		MatOfPoint2f destMat = new MatOfPoint2f();
		Point[] points = finderCenters.getArray();
		Point[] destPoints = new Point[3];
		
		codeWidth = Core.norm(OpenCVExtras.subtractPoints2D(points[1], points[2]), Core.NORM_L2) / Math.sqrt(2);
		
		srcMat.fromArray(points);
		destPoints[0] = new Point(Math.round(points[0].x), Math.round(points[0].y));
		destPoints[1] = new Point(Math.round(points[0].x), Math.round(points[0].y + codeWidth));
		destPoints[2] = new Point(Math.round(points[0].x + codeWidth), Math.round(points[0].y));
		
		destMat.fromArray(destPoints);
		
		return Imgproc.getAffineTransform(srcMat, destMat);
	}
	
	public static Mat affineImg(Mat inImg, Mat affineMat) {
		Mat outImg = new Mat(inImg.size(), CvType.CV_8UC1);
		
		Imgproc.warpAffine(inImg, outImg, affineMat, inImg.size(), Imgproc.INTER_LINEAR);
		
		return outImg;
	}
	
	public static QRFinderPattern affineFinderPatterns(QRFinderPattern finderCenters, Mat affineMat) {
		Point[] points = finderCenters.getArray();
		
		for(int i = 0; i < 3; i++) {
			points[i] = affinePoint(points[i], affineMat);
		}
		
		finderCenters.setValue(points);
		
		return finderCenters;
	}
	
	public static Point affinePoint(Point myPoint, Mat affineMat) {
		Mat ptTrans = new Mat(3, 1, CvType.CV_64FC1);
		
		Core.gemm(affineMat, OpenCVExtras.pointToMat3D(myPoint), 1.0, new Mat(), 0.0, ptTrans);
		
		return OpenCVExtras.mat3DToPoint(ptTrans);
	}
	
	/**
	 * <p>
	 * Gets a perspective matrix associated with a set of finder patterns
	 * and a list of alignment points
	 * </p>
	 * 
	 * @param inImg Image matrix to process
	 * @param finderCenters An instance of the three finder patterns of a QR Code
	 * @param alignPoints A list of alignment pattern centers
	 * @param modWidth The estimated module width
	 * @return A perspective matrix
	 */
	public static Mat getPerspectiveMat(QRFinderPattern finderCenters, List <Point> alignPoints, double modWidth) {
		int ver;
		double alignDist;
		double codeWidth;
		double closestDist = 1000000;
		int[] alignLoc = {0, 18, 22, 26, 30, 34};
		Point bestLR = new Point();
		Point ptLR = new Point();
		MatOfPoint2f srcMat = new MatOfPoint2f();
		MatOfPoint2f destMat = new MatOfPoint2f();
		
		ver = QRDataDecode.predictVersion(finderCenters, modWidth);
		codeWidth = modWidth * (4 * (double) ver + 10);
		
		if(ver > 6) {
			return null;
		}
		
		ptLR = new Point(finderCenters.getPosition(QRFinderPattern.POS_UL).x + (alignLoc[ver - 1] - 3) * modWidth, finderCenters.getPosition(QRFinderPattern.POS_UL).y + (alignLoc[ver - 1] - 3) * modWidth);
		
		for(Point iPoint : alignPoints) {
			alignDist = Core.norm(OpenCVExtras.subtractPoints2D(iPoint, ptLR));
			
			if(OpenCVExtras.compDouble(alignDist, closestDist, OpenCVExtras.OP_LT, 1000)) {
				closestDist = alignDist;
				bestLR = iPoint;
			}
		}
		
		// Just retain the alignment pattern position if the closest point
		// really closely matches the predicted position. Allow a tolerance
		// of 5 pixels
		if(OpenCVExtras.compDouble(closestDist, 5, OpenCVExtras.OP_LT, 1000)) {
			bestLR = ptLR;
		}
		
		srcMat.fromArray(finderCenters.getPosition(QRFinderPattern.POS_UL), 
				finderCenters.getPosition(QRFinderPattern.POS_UR),
				finderCenters.getPosition(QRFinderPattern.POS_LL),
				bestLR);
		
		destMat.fromArray(new Point((int) finderCenters.getPosition(QRFinderPattern.POS_UL).x, (int) finderCenters.getPosition(QRFinderPattern.POS_UL).y),
				new Point((int) (finderCenters.getPosition(QRFinderPattern.POS_UL).x + codeWidth), (int) finderCenters.getPosition(QRFinderPattern.POS_UL).y),
				new Point((int) finderCenters.getPosition(QRFinderPattern.POS_UL).x, (int) (finderCenters.getPosition(QRFinderPattern.POS_UL).y + codeWidth)),
				ptLR);
		
		return Imgproc.getPerspectiveTransform(srcMat, destMat);
	}
	
	public static Mat perspImg(Mat inImg, Mat perspMat) {
		Mat outImg = new Mat(inImg.size(), CvType.CV_8UC1);
		
		Imgproc.warpPerspective(inImg, outImg, perspMat, inImg.size(), Imgproc.INTER_NEAREST);
		
		return outImg;
	}
	
	public static QRFinderPattern perspFinderPatterns(QRFinderPattern finderCenters, Mat perspMat) {
		Point[] myCenters = finderCenters.getArray();
		
		for(int i = 0; i < 3; i++) {
			myCenters[i] = perspPoint(myCenters[i], perspMat);
		}
		
		finderCenters.setValue(myCenters);
		
		return finderCenters;
	}
	
	public static Point perspPoint(Point myPoint, Mat perspMat) {
		Mat ptTrans = new Mat(3, 1, CvType.CV_64FC1);
		
		Core.gemm(perspMat, OpenCVExtras.pointToMat3D(myPoint), 1.0, new Mat(), 0.0, ptTrans);
		
		return OpenCVExtras.mat3DToPoint(ptTrans);
	}
}
